#include "CommandProcessing.h"
#include "inc/hw_ints.h"
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "driverlib/uart.h"
#include "utils/uartstdio.h"
#include "utils/crc.h"
#include "myADC.h"
#include "driverlib/gpio.h"
#include "inc/lm4f232h5qd.h"

void ProcessPacket(char* packet, unsigned char currentMode)
{
	
	unsigned long ulLoop;
	unsigned char pin;
	//Packet is the incoming packet, currentMode is the current state the controller is in
	unsigned short OpCode = packet[1] + (packet[0] << 8);  //Get the opcode from the packet - this may be flipping the bytes -need to verify
	unsigned char packetMode = (OpCode & 0xE000) >> 13; //Get Top 3 bits of opcode
		//Print out the packet with the ACK bit flipped and a new CRC
		 char outputPacket[10]; //Make this the maximum output packet size
		unsigned short packetCRC;
		unsigned short outputOpcode = (OpCode | 0x1000); //Set the ACK bit high
		unsigned long ADCvalue[4];


	if(currentMode != packetMode)
	{
		//Do Mode transition stuff
	}
	if(packetMode == 1) //Synchronization packet
	{

		outputPacket[0] = (char)(outputOpcode >> 8);
		outputPacket[1] = (char)(outputOpcode & 0x00FF);
		outputPacket[2] = (char)0;
		packetCRC = Crc16(0, (const unsigned char *)outputPacket, 3);
		outputPacket[3] = (char)(packetCRC & 0x00FF);
		outputPacket[4] = (char)(packetCRC >> 8);
		
		UARTwrite(outputPacket, 5); 
		}
	else if(packetMode==2) //Single Sample Packet
	{
			switch(packet[1]) //This is the function byte
			{
				case 1:  //This is the AI Read Pin Case
					//Test: Right now lets just return some data
					outputPacket[0] = (char)(outputOpcode >> 8);
					outputPacket[1] = (char)(outputOpcode & 0x00FF);

					outputPacket[2] = (char)2; //length of data
					EnableADC();
					
					ReadADC(ADCvalue);


					outputPacket[3] =  (char)(ADCvalue[0] & 0x00FF);
					outputPacket[4] =  (char)(ADCvalue[0] >> 8);
					 
					packetCRC = Crc16(0, (const unsigned char *)outputPacket, 5);
					outputPacket[5] = (char)(packetCRC & 0x00FF);
					outputPacket[6] = (char)(packetCRC >> 8);
		
					UARTwrite(outputPacket, 7); 
					break;

				case 2:  //This is the Read Accelerometer Data case

					//Test: Right now lets just return some data
					outputPacket[0] = (char)(outputOpcode >> 8);
					outputPacket[1] = (char)(outputOpcode & 0x00FF);

					outputPacket[2] = (char)6; //length of data
					EnableADC();
					
					ReadADC(ADCvalue);


					outputPacket[3] =  (char)(ADCvalue[0] & 0x00FF);
					outputPacket[4] =  (char)(ADCvalue[0] >> 8);
					outputPacket[5] =  (char)(ADCvalue[1] & 0x00FF);
					outputPacket[6] =  (char)(ADCvalue[1] >> 8);
					outputPacket[7] =  (char)(ADCvalue[2] & 0x00FF);
					outputPacket[8] =  (char)(ADCvalue[2] >> 8);
										 
					packetCRC = Crc16(0, (const unsigned char *)outputPacket, 9);

					outputPacket[9] = (char)(packetCRC & 0x00FF);
					outputPacket[10] = (char)(packetCRC >> 8);
		
					UARTwrite(outputPacket, 11); 
					break;
				
				case 3:	// DO
					switch(packet[3]){ // Sets port
						case 0:
							break;
						case 1:
							SYSCTL_RCGC2_R = SYSCTL_RCGC2_GPIOB;
				    	   	ulLoop = SYSCTL_RCGC2_R;
																			
    						GPIO_PORTB_DIR_R = packet[4];
    						GPIO_PORTB_DEN_R = packet[4];
						
				        	GPIO_PORTB_DATA_R |= packet[4];
							break;
						case 2:
							SYSCTL_RCGC2_R = SYSCTL_RCGC2_GPIOC;
				    	   	ulLoop = SYSCTL_RCGC2_R;
																			
    						GPIO_PORTC_DIR_R = packet[4];
    						GPIO_PORTC_DEN_R = packet[4];
						
				        	GPIO_PORTC_DATA_R |= packet[4];
							break;
						case 3:
							SYSCTL_RCGC2_R = SYSCTL_RCGC2_GPIOD;
				    	   	ulLoop = SYSCTL_RCGC2_R;
																			
    						GPIO_PORTD_DIR_R = packet[4];
    						GPIO_PORTD_DEN_R = packet[4];
						
				        	GPIO_PORTD_DATA_R |= packet[4];
							
							break;
					    default:
							break;
						}
						outputPacket[0] = (char)(outputOpcode >> 8);
						outputPacket[1] = (char)(outputOpcode & 0x00FF);
						outputPacket[2] = (char)0; //length of data							
						packetCRC = Crc16(0, (const unsigned char *)outputPacket, 9);
						outputPacket[3] = (char)(packetCRC & 0x00FF);
						outputPacket[4] = (char)(packetCRC >> 8);
		
						UARTwrite(outputPacket, 5);
				
				default:
					break;
			}
	}	
}
